#include "simple_dataframe_slave.h"
#include "data_holder.h"
#include <stdio.h>

Simple_dataframe::Simple_dataframe(Stream* _trans): trans(_trans){
    recv_state = STATE_RECV_FIX;
    memset(nf, 0, sizeof(nf));
}

bool Simple_dataframe::init(){
    return true;
}

int Simple_dataframe::data_recv(unsigned char* c, unsigned short len){
    struct Message* pMsg = (struct Message*)c;
    unsigned char data_len = 0;
    unsigned char check = 0;
    int ret = 0;
    
    if(pMsg->head.flag == FIX_HEAD)
    {
        if(pMsg->head.msg_id < ID_MESSGAE_MAX)
        {
            data_len = pMsg->head.length;
            /*去掉3字节帧头和1字节校验和1字节帧尾等于数据长度*/
            if(len-5 >= data_len)
            {
                if(pMsg->data[data_len + 1] == FIX_END)
                {
                    int i = 0;
                    /* 计算校验和 */
                    for(i=0;i<data_len+sizeof(struct Head);i++)
                        check+=*(c+i);
                    
                    if(check == pMsg->data[data_len])
                    {
                        /*
                        void *memcpy(void *destin, void *source, unsigned n);
                        copy from source to dest
                        */
                        active_rx_msg.head.flag = pMsg->head.flag;
                        active_rx_msg.head.msg_id = pMsg->head.msg_id;
                        active_rx_msg.head.length = pMsg->head.length;
                        if(data_len != 0)
                            memcpy(active_rx_msg.data, pMsg->data, data_len);
                        
                        ret = data_len+5;
                        
                    }
                    else
                        ret = 0;
                }
            }
        }
    }

    //返回已读的数据长度
    return ret;
}

bool Simple_dataframe::data_recv(unsigned char c){
    //printf("%02x", c);
    switch (recv_state){
    case STATE_RECV_FIX:
        if (c == FIX_HEAD){
            memset(&active_rx_msg,0, sizeof(active_rx_msg));
            active_rx_msg.head.flag = c;
            active_rx_msg.check += c;

            recv_state = STATE_RECV_ID;
        }
        break;
    case STATE_RECV_ID:
        if (c < ID_MESSGAE_MAX){
            active_rx_msg.head.msg_id = c;
            active_rx_msg.check += c;
            recv_state = STATE_RECV_LEN;
        }
        else
            recv_state = STATE_RECV_FIX;
        break;
    case STATE_RECV_LEN:
        active_rx_msg.head.length =c;
        active_rx_msg.check += c;
        if (active_rx_msg.head.length==0)
            recv_state = STATE_RECV_CHECK;
        else
            recv_state = STATE_RECV_DATA;
        break;
    case STATE_RECV_DATA:
        active_rx_msg.data[active_rx_msg.recv_count++] = c;
        active_rx_msg.check += c;
        if (active_rx_msg.recv_count >=active_rx_msg.head.length)
            recv_state  = STATE_RECV_CHECK;
        break;
    case STATE_RECV_CHECK:
        recv_state = STATE_RECV_FIX;
        if (active_rx_msg.check == c){
            return true;
        }
        break;
    default:
        recv_state = STATE_RECV_FIX;
    }

    return false;
}

bool Simple_dataframe::data_parse(){
    MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.head.msg_id;

    Data_holder* dh = Data_holder::get();
    switch (id){
    case ID_GET_VERSION:
        send_message(id, (unsigned char*)&dh->firmware_info, sizeof(dh->firmware_info));
        break;
    case ID_SET_ROBOT_PARAMTER:
        memcpy(&dh->parameter, active_rx_msg.data, sizeof(dh->parameter));
        send_message(id);
        break;
    case ID_GET_ROBOT_PARAMTER:
        send_message(id, (unsigned char*)&dh->parameter, sizeof(dh->parameter));
        break;
    case ID_CLEAR_ODOM:
        send_message(id);
        break;
    case ID_SET_VELOCITY:
        memcpy(&dh->velocity, active_rx_msg.data, sizeof(dh->velocity));
        send_message(id);
        break;
    case ID_GET_ODOM:
        send_message(id, (unsigned char*)&dh->odom, sizeof(dh->odom));
        break;
    case ID_GET_PID_DATA:
        send_message(id, (unsigned char*)&dh->pid_data, sizeof(dh->pid_data));
        break;
    case ID_GET_IMU_DATA:
        send_message(id, (unsigned char*)&dh->imu_data, sizeof(dh->imu_data));
        break;
    case ID_SET_EACH_MOTOR:
        memcpy(&dh->each_motor, active_rx_msg.data, sizeof(dh->each_motor));
        send_message(id);
        break;
    case ID_SET_HYDRAU_RAISE:
        memcpy(&dh->hydrau_raise, active_rx_msg.data, sizeof(dh->hydrau_raise));
        send_message(id);
        break;
    case ID_SET_POWER:
        memcpy(&dh->power_onoff, active_rx_msg.data, sizeof(dh->power_onoff));
        send_message(id);
        break;
    case ID_GET_ROBOT_STATE:
        pushRobotState();
        break;
    case ID_RESET_WHEELS:
        send_message(id);
        break;
    case ID_SET_SPIN:
        memcpy(&dh->robot_spin, active_rx_msg.data, sizeof(dh->robot_spin));
        send_message(id);
        break;
    case ID_SET_DOUBLE_CAR:
        memcpy(&dh->double_car, active_rx_msg.data, sizeof(dh->double_car));
        send_message(id);
        break;
    case ID_SET_ODOM_TF:
        memcpy(&dh->odom_tf, active_rx_msg.data, sizeof(dh->odom_tf));
        send_message(id);
        break;
    default:
        break;
    }

    if (id < ID_MESSGAE_MAX && nf[id] != 0)
        nf[id]->update(id, dh);

    return true;
}

bool Simple_dataframe::send_message(const MESSAGE_ID id){
    Message msg(id);

    send_message(&msg);

    return true;
}

bool Simple_dataframe::send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len){
    Message msg(id, data, len);

    send_message(&msg);

    return true;
}

bool Simple_dataframe::send_message(Message* msg){
    if (trans == 0)
        return true;
    
    trans->writeBytes((unsigned char*)msg, sizeof(msg->head)+msg->head.length+2);//plus check and FixEnd

    return true;
}


void Simple_dataframe::pushRobotState()
{
    Data_holder* dh = Data_holder::get();
    this->send_message(ID_GET_ROBOT_STATE, (unsigned char*)&dh->robot_state, sizeof(dh->robot_state));
}

/* EOF */
